/*
 * @Date: 2020-10-05 03:36:25
 * @LastEditors: Tonser
 * @LastEditTime: 2020-10-05 03:53:46
 * @FilePath: /swarm_ws/src/px4_swarm_control/src/test/test_uwb_ros_node.cpp
 * @Description: test wub ros node
 */
#include "UWBRosNode.h"
#include "../../include/Utils/RosUtils.h"
// test

Buffer current_buffer;
int MAX_BUFFER_SIZE = 100;

int main(int argc, char **argv) {
    ROS_INFO("INF UWB ROS Initing\n");
    ros::init(argc, argv, "uwb_node");
    ros::NodeHandle nh("uwb_node");
    int baudrate = 921600;
    std::string serial_name;
    bool enable_debug = false;
    baudrate = RosUtils::readParam<int>(nh,"baudrate",921600);
    enable_debug = RosUtils::readParam<bool>(nh,"enable_debug_print",false);
    serial_name = RosUtils::readParam<std::string>(nh,"serial_name","/dev/ttyUSB0");
    UWBRosNode *uwb_ros_node = UWBRosNode::getInstance();
    uwb_ros_node->setUWBRosNode(serial_name, baudrate, nh, enable_debug);
    ros::AsyncSpinner spinner(3);
    spinner.start();
    ros::waitForShutdown();
}